#include "ros/ros.h"
#include "can_msgs/Frame.h"
#include "Cybergear_control.hpp"
#include "Saber_control.hpp"

double motor1_stop=-1.1587;
double motor2_stop=-0.4731;
double motor3_stop=-0.7146;
double motor4_stop=0.6412;
double motor5_stop=0;//代码中会自动更新该参数
double motor6_stop=0;//代码中会自动更新该参数


int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");

    ros::init(argc,argv,"Saber_stop");

    ros::NodeHandle nh;

    ros::Publisher roscan_send_message = nh.advertise<can_msgs::Frame>("sent_messages",10);

    //延时1秒确保socketcan节点完全启动才可以can通信
    sleep(1);

    //机械臂回归终止状态
    location_mode_write(motor1_id, master1_id,0.5,motor1_stop,roscan_send_message);
    sleep(0.1);
    location_mode_write(motor2_id, master1_id,0.5,motor2_stop,roscan_send_message);    
    sleep(0.1);
    location_mode_write(motor3_id, master1_id,0.5,motor3_stop,roscan_send_message);
    sleep(0.1);
    location_mode_write(motor4_id, master1_id,0.5,motor4_stop,roscan_send_message);
    sleep(2);
    //发送指令让6个电机停止
    motor_stop(1,0,roscan_send_message);
    motor_stop(2,0,roscan_send_message);
    motor_stop(3,0,roscan_send_message);
    motor_stop(4,0,roscan_send_message);
    motor_stop(5,0,roscan_send_message);
    motor_stop(6,0,roscan_send_message);

    // Saber saber;
    // double target_pose[16]={-0.8422,-0.5383,0.0291,286.3178,
    //                         -0.5389,0.8394,-0.0707,-74.0822,
    //                         0.0136,-0.0753,-0.9971,-14.8036,
    //                         0,0,0,1};
    // uint8_t solution_num=saber.inverse_kinematics_solver(target_pose);
    // if(solution_num==0) ROS_INFO("Fail...");
    // else    
    // {
    //     ROS_INFO("Success!!!");
    //     ROS_INFO("%f %f %f %f %f %f",saber.theta[0],saber.theta[1],saber.theta[2],saber.theta[3],saber.theta[4],saber.theta[5]);
    // }
    while(ros::ok())
    {
        // ROS_INFO("机械臂电机已停止!");
        // ros::Duration(2).sleep();
    }

    return 0;
}
